# -*- coding: utf-8 -*-
'''
Created on Jul 3, 2019

@author: yl
'''
from FFT_Interpolation import *
import cv2
from mpl_toolkits.mplot3d import Axes3D
from scipy import signal
from scipy.optimize import curve_fit
from scipy.signal import *

j = complex(0, 1)
c = 3e8 # 光速 [m/s]
Lamda = 633e-9 # 光波长 [m]
Fc = c / Lamda # 光频率 [Hz]

pix_size = 5.3e-6
pix_num = 1000
screen_diameter = pix_num * pix_size



if 0: ### Simulated Line Signal
    if 0: ### single line test
        x = np.linspace(0, (pix_num-1)*pix_size, pix_num)
        tau0 = pix_size
        fs = 1/tau0
        N = len(x)
        f = 8000.0
        phi = 0
    #     sig = np.cos(2*np.pi*f*x + phi) + 0.8 * np.cos(2*np.pi*2.1*f*x + phi) + 0.5 * np.cos(2*np.pi*0.15*f*x + phi)
        sig = np.cos(2*np.pi*f*x + phi)
#         sig *= signal.gaussian(N, std=N/10)
#         sig += np.random.normal(0, 2, len(sig))
#         sig_AC = np.diff(sig)

        frequency_estim, phase_estim, freqline_estim, magnetude_estim, frequency_used = FFT_interpolation_boxcar(sig, pix_size)
        print(frequency_estim, phase_estim)
        Period_num = frequency_estim * len(sig) * pix_size
        N_sel = int(len(sig) * int(Period_num) / Period_num)
        sig_sel = sig[:N_sel+1]
        freqline_sel, sig_FFT_sel, sig_freq_sel, sig_phase_sel = FFT_cal(sig_sel, pix_size)
        
        plt.figure('Simulated Line Signal')
        plt.gcf().set_size_inches(12,12)
        plt.subplot(3,1,1)
        plt.title("Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(sig, 'b')
#         plt.plot(sig_AC, 'r')
        plt.grid(which='major', axis='both')
        plt.subplot(3,1,2)
        plt.title("Freq. Estimation")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(freqline_estim, magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.plot([frequency_estim, frequency_estim], [0,np.max(magnetude_estim)], label='Estimated Freq.', color='red')
#         plt.xlim(0,5000)
        plt.grid(which='major', axis='both')
        plt.subplot(3,1,3)
        plt.title("Recalculated FFT")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(freqline_sel, sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
#         plt.xlim(0,5000)
        plt.grid(which='major', axis='both')
        plt.tight_layout()
        plt.show()

    if 1: ### Multiline test
        x = np.linspace(0, (pix_num-1)*pix_size, pix_num)
        tau0 = pix_size
        fs = 1/tau0
        N = len(x)
        
        frequency_estim_set = []
        frequency_estim_2_set = []
        f_error = []
        f_error_2 = []
        phi_error = []
        phi_error_2 = []
        f_used_error = []
        
        
        ### Scanning Define
        f_num = 10000
        phi_num = f_num
        
        f = np.linspace(2000,10000,f_num)
        phi = np.linspace(0,0,phi_num)
#         f = np.linspace(7000,7000,f_num)
#         phi = np.linspace(0,4*np.pi,phi_num)

        for i in range(len(f)):
            sig = np.cos(2*np.pi*f[i]*x + phi[i])
            window = signal.gaussian(len(sig), std=len(sig)/10)
            sig *= window

            frequency_estim, phase_estim, freqline_estim, magnetude_estim, frequency_used = FFT_interpolation_boxcar(sig, pix_size)
            Period_num = frequency_estim * len(sig) * pix_size
            N_sel = int(len(sig) * int(Period_num) / Period_num)
            sig_sel = sig[:N_sel+1]
            frequency_estim_2, phase_estim_2 = FFT_interpolation_boxcar(sig_sel, pix_size)[0:2]
            
            
#             print(frequency_estim, phase_estim)
            frequency_estim_set.append(frequency_estim)
            frequency_estim_2_set.append(frequency_estim_2)
            f_error.append(frequency_estim-f[i])
            f_error_2.append(frequency_estim_2-f[i])
            phi_error.append(phase_estim-phi[i])
            phi_error_2.append(phase_estim_2-phi[i])
            f_used_error.append(frequency_used-f[i])
            
            
            ### Show one Frame
            if i == 0:
#                 frequency_estim, phase_estim, freqline_estim, magnetude_estim, frequency_used = FFT_interpolation_boxcar(sig, pix_size)
                print(frequency_estim, phase_estim)
                Period_num = frequency_estim * len(sig) * pix_size
                N_sel = int(len(sig) * int(Period_num) / Period_num)
                sig_sel = sig[:N_sel+1]
                freqline_sel, sig_FFT_sel, sig_freq_sel, sig_phase_sel = FFT_cal(sig_sel, pix_size)
                plt.figure('Simulated Line Signal')
                plt.gcf().set_size_inches(12,12)
                plt.subplot(3,1,1)
                plt.title("Signal Cutting")
                plt.ylabel("Amplitude")
                plt.xlabel("Samples")
                plt.plot(sig, 'b')
                plt.plot(sig_sel, 'r')
                plt.grid(which='major', axis='both')
                plt.subplot(3,1,2)
                plt.title("Freq. Estimation")
                plt.ylabel("Magnitude")
                plt.xlabel("Frequency")
                plt.stem(freqline_estim, magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
                plt.plot([frequency_estim, frequency_estim], [0,np.max(magnetude_estim)], label='Estimated Freq.', color='red')
        #         plt.xlim(0,5000)
                plt.grid(which='major', axis='both')
                plt.subplot(3,1,3)
                plt.title("Recalculated FFT")
                plt.ylabel("Magnitude")
                plt.xlabel("Frequency")
                plt.stem(freqline_sel, sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        #         plt.xlim(0,5000)
                plt.grid(which='major', axis='both')
                plt.tight_layout()
#                 plt.show()
        print(1)
        plt.figure('Multiline Test')
        plt.subplot(3,1,1)
        if f[0] != f[-1]:
            plt.plot(f, frequency_estim_set)
        elif phi[0] != phi[-1]:
            plt.plot(phi, frequency_estim_set)
        else:
            raise SystemExit("Error! Check f or phi range")
        plt.grid(which='major', axis='both')
        plt.subplot(3,1,2)
        if f[0] != f[-1]:
            plt.plot(f, f_error, 'b')
#             plt.plot(f, f_used_error, 'r')
            plt.plot(f, f_error_2, 'k')
        elif phi[0] != phi[-1]:
            plt.plot(phi, f_error, 'b')
            plt.plot(phi, f_error_2, 'k')
#             plt.plot(phi, f_used_error, 'r')
        else:
            raise SystemExit("Error! Check f or phi range")
        plt.grid(which='major', axis='both')
        plt.subplot(3,1,3)
        if f[0] != f[-1]:
            plt.plot(f, np.unwrap(phi_error))
            plt.plot(f, np.unwrap(phi_error_2))
        elif phi[0] != phi[-1]:
            plt.plot(phi, np.unwrap(phi_error))
            plt.plot(phi, np.unwrap(phi_error_2))
        else:
            raise SystemExit("Error! Check f or phi range")
        plt.grid(which='major', axis='both')
        plt.tight_layout()
        
if 1: #simulated pattern
    # Generate Pattern
    I_0 = 1
    dx = np.linspace(0, (pix_num-1)*pix_size, num=pix_num)
    dy = dx
    V_x, V_y, Vz = 0.000, 0.000, 1 # Reference
    V_x_2, V_y_2, Vz_2 = 0.003, 0.003, 1 # Measurement
    tana = (V_x**2 + V_y**2)**0.5
    tana_2 = (V_x_2**2 + V_y_2**2)**0.5
    L, D = 0.2, 0
    X, Y = np.meshgrid(dx, dy)
    D_d = D + X * (V_x_2 - V_x) + Y * (V_y_2 - V_y)
    L_d = L + X * V_x + Y * V_y
    diff_L_d_2 = D_d + (L_d + D_d) * (1 - (V_x_2**2 + V_y_2**2)) / (1 + (V_x_2**2 + V_y_2**2)) - L_d * (1 - (V_x**2 + V_y**2)) / (1 + (V_x**2 + V_y**2))
    Z = 2 * I_0 * (1 + np.cos(2 * np.pi * diff_L_d_2 / Lamda))
    Z_sel = Z #Selected Area
    plt.figure('Simulated Pattern')
    plt.subplot(1,2,1)
    plt.title("Whole Frame")
    plt.imshow(Z, cmap='gray')
    plt.subplot(1,2,2)
    plt.title("Selected Area")
    plt.imshow(Z_sel, cmap='gray')
    
    if 1:    #Analyzing one line
        hor_data, ver_data = Z_sel[int(len(Z_sel)/2), :], Z_sel[:, int(len(Z_sel[1])/2)]
        
        
        hor_frequency_estim, hor_phase_estim, hor_freqline_estim, hor_magnetude_estim, hor_frequency_used = FFT_interpolation_boxcar(hor_data, pix_size)
        ver_frequency_estim, ver_phase_estim, ver_freqline_estim, ver_magnetude_estim, ver_frequency_used = FFT_interpolation_boxcar(ver_data, pix_size)
        print(hor_frequency_estim, ver_frequency_estim)
        hor_Period_num, ver_Period_num = hor_frequency_estim * len(hor_data) * pix_size, ver_frequency_estim * len(ver_data) * pix_size
        hor_N_sel, ver_N_sel = int(len(hor_data) * int(hor_Period_num) / hor_Period_num), int(len(ver_data) * int(ver_Period_num) / ver_Period_num)
        hor_data_sel, ver_data_sel = hor_data[:hor_N_sel+1], ver_data[:ver_N_sel+1]
        
#         zero_padd_hor, zero_padd_ver = np.zeros(4096-len(hor_data_sel)), np.zeros(4096-len(ver_data_sel))
#         hor_data_sel = np.concatenate((hor_data_sel,zero_padd_hor))
#         ver_data_sel = np.concatenate((ver_data_sel,zero_padd_ver))
        
        hor_freqline_sel, hor_sig_FFT_sel, hor_sig_freq_sel, hor_sig_phase_sel = FFT_cal(hor_data_sel, pix_size)
        ver_freqline_sel, ver_sig_FFT_sel, ver_sig_freq_sel, ver_sig_phase_sel = FFT_cal(ver_data_sel, pix_size)
    
        plt.figure('Line Test')
        plt.gcf().set_size_inches(12,12)
        plt.subplot(3,2,1)
        plt.title("Horizontal Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(hor_data, 'b')
        plt.plot(hor_data_sel, 'r')
        plt.grid(which='major', axis='both')
        plt.subplot(3,2,3)
        plt.title("Horizontal Freq. Estimation")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(hor_freqline_estim, hor_magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.plot([hor_frequency_estim, hor_frequency_estim], [0,np.max(hor_magnetude_estim)], label='Estimated Freq.', color='red')
        plt.grid(which='major', axis='both')
        plt.xlim(0,8000)
        plt.subplot(3,2,5)
        plt.stem(hor_freqline_sel, hor_sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.title("Horizontal Recalculated FFT")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.xlim(0,8000)
        plt.grid(which='major', axis='both')
        
        plt.subplot(3,2,2)
        plt.title("Vertical Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(ver_data, 'b')
        plt.plot(ver_data_sel, 'r')
        plt.grid(which='major', axis='both')
        plt.subplot(3,2,4)
        plt.title("Vertical Freq. Estimation")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(ver_freqline_estim, ver_magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.plot([ver_frequency_estim, ver_frequency_estim], [0,np.max(ver_magnetude_estim)], label='Estimated Freq.', color='red')
        plt.grid(which='major', axis='both')
        plt.xlim(0,8000)
        plt.subplot(3,2,6)
        plt.stem(ver_freqline_sel, ver_sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.title("Vertical Recalculated FFT")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.grid(which='major', axis='both')
        plt.xlim(0,8000)
        
        plt.tight_layout()

    if 0: # Analyzing Frame
        hor_frequency_set = []
        ver_frequency_set = []
        hor_angle_set = []
        ver_angle_set = []
        hor_fft_set = []
        ver_fft_set = []
        theory_freq_X_set = []
        theory_freq_Y_set = []
        for i in range(len(Z_sel)):
            hor_data, ver_data= Z_sel[i, :], Z_sel[:, i]
            hor_data, ver_data = hor_data-np.average(hor_data), ver_data-np.average(ver_data)
            hor_frequency_estim, hor_phase_estim, hor_freqline_estim, hor_magnetude_estim, hor_frequency_used = FFT_interpolation_boxcar(hor_data, pix_size)
            ver_frequency_estim, ver_phase_estim, ver_freqline_estim, ver_magnetude_estim, ver_frequency_used = FFT_interpolation_boxcar(ver_data, pix_size)
#             print(hor_frequency_estim, ver_frequency_estim)
            hor_frequency_set.append(hor_frequency_estim)
            ver_frequency_set.append(ver_frequency_estim)
            hor_Period_num, ver_Period_num = hor_frequency_estim * len(hor_data) * pix_size, ver_frequency_estim * len(ver_data) * pix_size
            hor_N_sel, ver_N_sel = int(len(hor_data) * int(hor_Period_num) / hor_Period_num), int(len(ver_data) * int(ver_Period_num) / ver_Period_num)
            hor_data_sel, ver_data_sel = hor_data[:hor_N_sel], ver_data[:ver_N_sel]
            hor_freqline_sel, hor_sig_FFT_sel, hor_sig_freq_sel, hor_sig_phase_sel = FFT_cal(hor_data_sel, pix_size)
            ver_freqline_sel, ver_sig_FFT_sel, ver_sig_freq_sel, ver_sig_phase_sel = FFT_cal(ver_data_sel, pix_size)
            hor_fft_set.append(hor_sig_freq_sel)
            ver_fft_set.append(ver_sig_freq_sel)
            hor_angle_set.append(V_x-Lamda*hor_frequency_estim/2)
            ver_angle_set.append(V_y-Lamda*ver_frequency_estim/2)
#             theory_freq_X = (V_x_2*2-V_x*2) / (-Lamda)
            theory_freq_X = (V_x_2*2/(1+tana_2**2)-V_x*2/(1+tana**2)) / (-Lamda)
            theory_freq_Y = (V_y_2*2/(1+tana_2**2)-V_y*2/(1+tana**2)) / (-Lamda)
            theory_freq_X_set.append(theory_freq_X)
            theory_freq_Y_set.append(theory_freq_Y)
        hor_fft_set = np.array(hor_fft_set)
        ver_fft_set = np.array(ver_fft_set)

        fig = plt.figure('Frame Test')
        ax1 = fig.add_subplot(2,2,1, projection='3d')
        plt.title('Horizontal')
        X = np.linspace(hor_freqline_sel[0], hor_freqline_sel[-1], len(hor_freqline_sel))
        Y = np.linspace(1, len(hor_fft_set), len(hor_fft_set))
        X, Y = np.meshgrid(X[:30], Y)
        surf = ax1.plot_surface(X, Y, hor_fft_set[:,:30], rcount=300, ccount=300, cmap='jet')
        
        ax3 = fig.add_subplot(2,2,3)
        plt.title("Horizontal")
        plt.ylabel("Estimated Frequency [/m]")
        plt.xlabel("Line Number")
        plt.grid(which='major', axis='both')
        ax3.plot(hor_frequency_set, color='blue')
        ax3.plot(theory_freq_X_set, color='red')
        f_range = plt.gca().get_ylim()
        angle_range = (V_x-Lamda*f_range[0]/2, V_x-Lamda*f_range[1]/2)
        ax3_angle = ax3.twinx()
        plt.ylabel("Horizontal Angle [rad]")
        plt.ylim(angle_range)
        
        
        ax2 = fig.add_subplot(2,2,2, projection='3d')
        plt.title('Vertical')
        X = np.linspace(ver_freqline_sel[0], ver_freqline_sel[-1], len(ver_freqline_sel))
        Y = np.linspace(1, len(ver_fft_set), len(ver_fft_set))
        X, Y = np.meshgrid(X[:30], Y)
        surf = ax2.plot_surface(X, Y, ver_fft_set[:,:30], rcount=300, ccount=300, cmap='jet')
           
        ax4 = fig.add_subplot(2,2,4)
        plt.title("Vertical")
        plt.ylabel("Estimated Frequency [/m]")
        plt.xlabel("Line Number")
        plt.grid(which='major', axis='both')
        ax4.plot(ver_frequency_set, color='blue')
        ax4.plot(theory_freq_Y_set, color='red')
        f_range = plt.gca().get_ylim()
        angle_range = (V_y-Lamda*f_range[0]/2, V_y-Lamda*f_range[1]/2)
        ax4_angle = ax4.twinx()
        plt.ylabel("Vertical Angle [rad]")
        plt.ylim(angle_range)
        plt.tight_layout()
#     plt.show()
    
if 0: ### Photo Test
    pattern_path = '8V_10mHz_50fps_100s_2.bmp'
    img = cv2.imread(pattern_path, 0)
    size = img.shape
    print(size)
    plt.figure('Origin')
    plt.imshow(img, cmap='gray')
#     pattern_path = r'C:\Users\yu03\Desktop\Old Interferometer head 2019-07-10\Cleaned Optics\100mV_100mHz_50fps.avi'
    # pattern_path = r'C:\Users\yu03\Desktop\2019-06-26_18-39-10\Modulating(24cm)\X\x.avi'
    # pattern_path = '/Users/yl/Desktop/2019-06-26_18-39-10/Modulating/(24cm)/X/x.avi'
    # pattern_path = os.path.expanduser('~/Desktop/2019-06-26_18-39-10/Modulating/(24cm)/X/x.avi')
    # pattern_path = 'x.avi'
    
    cap = cv2.VideoCapture(pattern_path)
    frame_num = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
    print(frame_num, "Frames")
    for i in range(frame_num):
        ret, frame = cap.read() 
        img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    
    print(len(img))

#     fig = plt.figure('Spot')
#     ax = Axes3D(fig)
#     dx = np.linspace(1, len(img[0]), len(img[0]))
#     dy = np.linspace(1, len(img), len(img))
#     X, Y = np.meshgrid(dx, dy)
# #     norm = matplotlib.colors.Normalize(vmin=0, vmax=255)
#     surf = ax.plot_surface(X, Y, img, rcount=200, ccount=200, cmap='jet')
#     ax.set_zlim(0, 255)
#     plt.colorbar(surf, shrink=0.6)
    hor_threshold, ver_threshold = 40, 40
    
    if 0:    #Analyzing one line
        hor_data, ver_data = img[int(len(img)/2), :], img[:, int(len(img[1])/2)]
        hor_start, ver_start = np.where(hor_data > hor_threshold)[0][0], np.where(ver_data > ver_threshold)[0][0]
        hor_end, ver_end = np.where(hor_data > hor_threshold)[0][-1], np.where(ver_data > ver_threshold)[0][-1]
        hor_data_cut, ver_data_cut = hor_data[hor_start:hor_end], ver_data[ver_start:ver_end]
        hor_data_AC, ver_data_AC = hor_data_cut-np.average(hor_data_cut), ver_data_cut-np.average(ver_data_cut)
        hor_data_AC = hor_data_AC/signal.gaussian(len(hor_data_AC), std=len(hor_data_AC)/3)
        ver_data_AC = ver_data_AC/signal.gaussian(len(ver_data_AC), std=len(ver_data_AC)/3)
        hor_frequency_estim, hor_phase_estim, hor_freqline_estim, hor_magnetude_estim, hor_frequency_used = FFT_interpolation_boxcar(hor_data_AC, pix_size)
        ver_frequency_estim, ver_phase_estim, ver_freqline_estim, ver_magnetude_estim, ver_frequency_used = FFT_interpolation_boxcar(ver_data_AC, pix_size)
        hor_Period_num, ver_Period_num = hor_frequency_estim * len(hor_data_AC) * pix_size, ver_frequency_estim * len(ver_data_AC) * pix_size
        hor_N_sel, ver_N_sel = int(len(hor_data_AC) * int(hor_Period_num) / hor_Period_num), int(len(ver_data_AC) * int(ver_Period_num) / ver_Period_num)
        hor_data_sel, ver_data_sel = hor_data_AC[:hor_N_sel+1], ver_data_AC[:ver_N_sel+1]
#         zero_padd_hor, zero_padd_ver = np.zeros(4096-len(hor_data_sel)), np.zeros(4096-len(ver_data_sel))
#         hor_data_sel_padd = np.concatenate((hor_data_sel,zero_padd_hor))
#         ver_data_sel_padd = np.concatenate((ver_data_sel,zero_padd_ver))
        hor_freqline_sel, hor_sig_FFT_sel, hor_sig_freq_sel, hor_sig_phase_sel = FFT_cal(hor_data_sel, pix_size)
        ver_freqline_sel, ver_sig_FFT_sel, ver_sig_freq_sel, ver_sig_phase_sel = FFT_cal(ver_data_sel, pix_size)
        
        plt.figure('Real Line Test')
        plt.gcf().set_size_inches(12,12)
        plt.subplot(4,2,1)
        plt.title("Horizontal Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(hor_data, color='blue', label='Raw Data')
        x_sel = np.linspace(start=hor_start, stop=hor_end-1, num=hor_end-hor_start)
        plt.plot(x_sel, hor_data_cut, color='red', label='Cutted Data')
        legend = plt.legend(loc='upper right')
        plt.setp(legend.get_texts()[0], color = 'blue')
        plt.setp(legend.get_texts()[1], color = 'red')
        plt.grid(which='major', axis='both')
        plt.subplot(4,2,2)
        plt.title("Vertical Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(ver_data, color='blue', label='Raw Data')
        x_sel = np.linspace(start=ver_start, stop=ver_end-1, num=ver_end-ver_start)
        plt.plot(x_sel, ver_data_cut, color='red', label='Cutted Data')
        legend = plt.legend(loc='upper right')
        plt.setp(legend.get_texts()[0], color = 'blue')
        plt.setp(legend.get_texts()[1], color = 'red')
        plt.grid(which='major', axis='both')
        plt.subplot(4,2,3)
        plt.title("Horizontal Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(hor_data_AC, color='blue', label='Cuted Data (AC)')
        plt.plot(hor_data_sel, color='red', label='Selected Data')
        legend = plt.legend(loc='upper right')
        plt.setp(legend.get_texts()[0], color = 'blue')
        plt.setp(legend.get_texts()[1], color = 'red')
        plt.grid(which='major', axis='both')
        plt.subplot(4,2,4)
        plt.title("Vertical Signal Cutting")
        plt.ylabel("Amplitude")
        plt.xlabel("Samples")
        plt.plot(ver_data_AC, color='blue', label='Cuted Data (AC)')
        plt.plot(ver_data_sel, color='red', label='Selected Data')
        plt.grid(which='major', axis='both')
        legend = plt.legend(loc='upper right')
        plt.setp(legend.get_texts()[0], color = 'blue')
        plt.setp(legend.get_texts()[1], color = 'red')
        plt.subplot(4,2,5)
        plt.title("Horizontal Freq. Estimation")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(hor_freqline_estim, hor_magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.plot([hor_frequency_estim, hor_frequency_estim], [0,np.max(hor_magnetude_estim)], label='Estimated Freq.', color='red')
        plt.grid(which='major', axis='both')
        plt.xlim(0,10000)
        plt.subplot(4,2,6)
        plt.title("Vertical Freq. Estimation")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.stem(ver_freqline_estim, ver_magnetude_estim, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.plot([ver_frequency_estim, ver_frequency_estim], [0,np.max(ver_magnetude_estim)], label='Estimated Freq.', color='red')
        plt.grid(which='major', axis='both')
        plt.xlim(0,10000)
        
        plt.subplot(4,2,7)
        plt.stem(hor_freqline_sel, hor_sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.title("Horizontal Recalculated FFT")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.xlim(0,10000)
        plt.grid(which='major', axis='both')
        plt.subplot(4,2,8)
        plt.stem(ver_freqline_sel, ver_sig_freq_sel, linefmt='b', markerfmt='bo', basefmt=" ", use_line_collection=True)
        plt.title("Ver Recalculated FFT")
        plt.ylabel("Magnitude")
        plt.xlabel("Frequency")
        plt.xlim(0,10000)
        plt.grid(which='major', axis='both')
        
        plt.tight_layout()
        
    if 1:    #Analyzing Frame
        hor_line_num = []
        hor_freq_set = []
        hor_phi_set = []
        img = img[350:700, 500:870]
        plt.figure('selected area')
        plt.imshow(img, cmap='gray')
        for k in range(len(img)): ### horzontal lines
            data = img[k]
            print(k)
#             if len(np.where(data > hor_threshold)[0]) > 100:
            if 1:
#                 data_start = np.where(data > hor_threshold)[0][0]
#                 data_end = np.where(data > hor_threshold)[0][-1]
#                 data_cut = data[data_start:data_end]
                data_cut = data
                data_AC = data_cut-np.average(data_cut)
#                 data_AC = data_AC/signal.gaussian(len(data_AC), std=len(data_AC)/3)
                frequency_estim, phase_estim, freqline_estim, magnetude_estim, frequency_used = FFT_interpolation_boxcar(data_AC, pix_size)
                Period_num = frequency_estim * len(data_AC) * pix_size
                N_sel = int(len(data_AC) * int(Period_num) / Period_num)
                data_sel = data_AC[:N_sel+1]
#                 plt.figure('lineshape')
#                 plt.subplot(2,1,1)
#                 plt.plot(data)
#                 plt.subplot(2,1,2)
#                 plt.plot(data_AC, 'b')
#                 plt.plot(data_sel, 'r')
#                 plt.show()
#                 print(k, frequency_estim)
                hor_line_num.append(k)
                hor_freq_set.append(frequency_estim)
                hor_phi_set.append(phase_estim)
        
        plt.figure('Frame Test')
        plt.subplot(3,2,1)
        plt.plot(hor_line_num, hor_freq_set)
        plt.subplot(3,2,5)
        plt.plot(hor_line_num, np.unwrap(hor_phi_set))
        plt.subplot(3,2,3)
        measured_phase = np.unwrap(hor_phi_set)
        linear_fit = np.linspace(measured_phase[0], measured_phase[-1], len(measured_phase))
        nonlinearity = measured_phase - linear_fit
        plt.plot(hor_line_num, nonlinearity)
plt.show()